﻿/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include "SlamWebSocket.h"

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
** Implementation
*****************************************************************************/

RobotCmdNode::RobotCmdNode(SlamWebSocket* ws)
    : Node("slamwebsocket_node"), m_ws(ws)
{
    // m_pubCmd = this->create_publisher<pccmd_msg::msg::PcCmd>("PcCmdMsg",1);

    subscription_ = this->create_subscription<pccmd_msg::msg::PcCmd>(
        "PcCmdMsg", 10, std::bind(&RobotCmdNode::topic_callback, this, _1));

    m_pubSLAMReady =
        this->create_publisher<slamready_msg::msg::SlamReady>("SlamReady", 10);

    m_pubSLAMPos =
        this->create_publisher<slampos_msg::msg::SlamPos>("SlamPos", 10);
}

void RobotCmdNode::pubSlamReady()
{
    auto cmd = slamready_msg::msg::SlamReady();
    cmd.readystate = 1;
    m_pubSLAMReady->publish(cmd);
    RCLCPP_INFO(this->get_logger(), "pub m_pubSLAMReady");
}

void RobotCmdNode::pubSlamPos(float x, float y, float z, float rx, float ry, float rz)
{
    auto cmd = slampos_msg::msg::SlamPos();
    cmd.x = x;
    cmd.y = y;
    cmd.z = z;
    cmd.rx = rx;
    cmd.ry = ry;
    cmd.rz = rz;
    m_pubSLAMPos->publish(cmd);
    // RCLCPP_INFO(this->get_logger(), "pub m_pubSLAMReady x: %f, y: %f, z: %f", x, y, z);
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}

void RobotCmdNode::topic_callback(
    const pccmd_msg::msg::PcCmd::SharedPtr msg) const
{
    if (msg->cmdname == "getCloud") {
        m_ws->callGetCloud();
    } else if (msg->cmdname == "startLaser") {
        m_ws->callStartLaser(QString::fromStdString(msg->cmdcontent));
    } else if (msg->cmdname == "stopLaser") {
        m_ws->callStopLaser();
    }
}
